#include "HostComm.h"
#include "Driver/software/UART_XDS_PC/UART_XDS_PC.h"
#include <string.h>
#include <stdlib.h> // For atof

// 使用全局的UART_XDS_PC句柄进行通信
static UART_XDS_PC_Handle_t *g_host_uart_handle = &g_defaultXDS_PC;

// ==============================================================================
// 内部变量
// ==============================================================================

// 接收状态机状态
typedef enum {
    STATE_WAIT_HEADER_0,
    STATE_WAIT_HEADER_1,
    STATE_READ_CMD,
    STATE_READ_LEN,
    STATE_READ_DATA,
    STATE_READ_CHECKSUM
} RxState_t;

static RxState_t g_rx_state = STATE_WAIT_HEADER_0;
static uint8_t g_rx_buffer[128]; // 接收数据载荷的缓冲区
static uint8_t g_rx_cmd;
static uint8_t g_rx_len;
static uint8_t g_rx_count = 0;
static uint8_t g_calculated_checksum = 0;

// 回调函数指针
static MotorControlCallback   g_motor_control_callback = NULL;
static SetSpeedCallback       g_set_speed_callback = NULL;
static SetTargetYawCallback   g_set_target_yaw_callback = NULL;
static SetPIDParamsCallback   g_set_pid_params_callback = NULL;


// 内部函数声明
static uint8_t calculate_checksum(uint8_t cmd, const uint8_t *data, uint8_t length);
static void process_packet(void);
static void process_byte(uint8_t byte);

// ==============================================================================
// 回调函数注册实现
// ==============================================================================

void HostComm_RegisterMotorControlCallback(MotorControlCallback callback)
{
    g_motor_control_callback = callback;
}

void HostComm_RegisterSetSpeedCallback(SetSpeedCallback callback)
{
    g_set_speed_callback = callback;
}

void HostComm_RegisterSetTargetYawCallback(SetTargetYawCallback callback)
{
    g_set_target_yaw_callback = callback;
}

void HostComm_RegisterSetPIDParamsCallback(SetPIDParamsCallback callback)
{
    g_set_pid_params_callback = callback;
}


// ==============================================================================
// 发送功能实现 (与之前相同)
// ==============================================================================

/**
 * @brief 初始化HostComm驱动
 */
void HostComm_Init(void)
{
    // 底层UART (UART_XDS_PC) 已在 system_init() 中通过 UART_XDS_PC_InitDefault() 初始化。
    // 我们只需确保其工作在二进制模式而不是行模式。
    UART_XDS_PC_SetLineMode(g_host_uart_handle, false);
}

/**
 * @brief 计算给定数据载荷的校验和
 */
static uint8_t calculate_checksum(uint8_t cmd, const uint8_t *data, uint8_t length)
{
    uint8_t checksum = 0;
    checksum += cmd;
    checksum += length;
    for (uint8_t i = 0; i < length; i++) {
        checksum += data[i];
    }
    return checksum;
}

/**
 * @brief 发送一个完整的、符合协议的数据包到上位机
 * @param cmd 命令ID
 * @param data 指向数据载荷的指针
 * @param length 数据载荷的长度
 * @return HostComm_Status_t 操作状态
 */
HostComm_Status_t HostComm_SendPacket(uint8_t cmd, const uint8_t *data, uint8_t length)
{
    if (data == NULL && length > 0) {
        return HOSTCOMM_STATUS_INVALID_PARAM;
    }

    uint8_t packet_buffer[256]; // 临时发包缓冲区
    if (length > (sizeof(packet_buffer) - HOSTCOMM_MIN_PACKET_LEN)) {
        return HOSTCOMM_STATUS_ERROR; // 数据载荷过大
    }

    // 1. 构造包头
    packet_buffer[0] = HOSTCOMM_HEADER_0;
    packet_buffer[1] = HOSTCOMM_HEADER_1;
    packet_buffer[2] = cmd;
    packet_buffer[3] = length;

    // 2. 拷贝数据载荷
    if (length > 0) {
        memcpy(&packet_buffer[4], data, length);
    }

    // 3. 计算并添加校验和
    uint8_t checksum = calculate_checksum(cmd, data, length);
    packet_buffer[4 + length] = checksum;

    // 4. 通过UART发送完整数据包
    uint16_t packet_size = HOSTCOMM_MIN_PACKET_LEN - 1 + length + 1; // Header(2) + Cmd(1) + Len(1) + Data(N) + Checksum(1)
    UART_XDS_PC_Status_t tx_status = UART_XDS_PC_SendData(g_host_uart_handle, packet_buffer, packet_size);

    return (tx_status == UART_XDS_PC_STATUS_SUCCESS) ? HOSTCOMM_STATUS_SUCCESS : HOSTCOMM_STATUS_TX_FAIL;
}

/**
 * @brief 发送GPS数据
 */
HostComm_Status_t HostComm_SendGPS(float lon, float lat, float speed, uint8_t sats)
{
    HostComm_GPSData_t gps_data;
    gps_data.longitude = lon;
    gps_data.latitude = lat;
    gps_data.speed = speed;
    gps_data.satellites = sats;
    return HostComm_SendPacket(HOSTCOMM_CMD_GPS_DATA, (const uint8_t *)&gps_data, sizeof(gps_data));
}

/**
 * @brief 发送IMU（姿态）数据
 */
HostComm_Status_t HostComm_SendIMU(float pitch, float roll, float yaw)
{
    HostComm_IMUData_t imu_data;
    imu_data.pitch = pitch;
    imu_data.roll = roll;
    imu_data.yaw = yaw;
    return HostComm_SendPacket(HOSTCOMM_CMD_IMU_DATA, (const uint8_t *)&imu_data, sizeof(imu_data));
}

/**
 * @brief 发送磁力计（航向角）数据
 */
HostComm_Status_t HostComm_SendMag(float heading)
{
    HostComm_MagData_t mag_data;
    mag_data.heading = heading;
    return HostComm_SendPacket(HOSTCOMM_CMD_MAG_DATA, (const uint8_t *)&mag_data, sizeof(mag_data));
}

/**
 * @brief 发送小车状态数据
 */
HostComm_Status_t HostComm_SendVehicleStatus(int16_t motorL, int16_t motorR, float battery)
{
    HostComm_VehicleStatus_t status_data;
    status_data.motorL = motorL;
    status_data.motorR = motorR;
    status_data.battery_voltage = battery;
    return HostComm_SendPacket(HOSTCOMM_CMD_VEHICLE_STATUS, (const uint8_t *)&status_data, sizeof(status_data));
}

/**
 * @brief 发送系统心跳
 */
HostComm_Status_t HostComm_SendHeartbeat(bool mpu_ok, bool hmc_ok, bool gps_ok)
{
    HostComm_SystemHeartbeat_t heartbeat_data;
    heartbeat_data.mpu_status = mpu_ok ? 1 : 0;
    heartbeat_data.hmc_status = hmc_ok ? 1 : 0;
    heartbeat_data.gps_status = gps_ok ? 1 : 0;
    return HostComm_SendPacket(HOSTCOMM_CMD_SYSTEM_HEARTBEAT, (const uint8_t *)&heartbeat_data, sizeof(heartbeat_data));
}

/**
 * @brief 发送调试字符串
 */
HostComm_Status_t HostComm_SendDebugString(const char *str)
{
    if (str == NULL) {
        return HOSTCOMM_STATUS_INVALID_PARAM;
    }
    return HostComm_SendPacket(HOSTCOMM_CMD_DEBUG_LOG, (const uint8_t *)str, strlen(str));
}

// ==============================================================================
// 接收与解析功能实现
// ==============================================================================

/**
 * @brief 处理接收到的有效数据包
 */
static void process_packet(void)
{
    // 根据命令ID调用相应的回调函数
    switch (g_rx_cmd) {
        case HOSTCOMM_CMD_MOTOR_CONTROL:
            if (g_motor_control_callback && g_rx_len == 1) {
                g_motor_control_callback(g_rx_buffer[0]);
            }
            break;

        case HOSTCOMM_CMD_SET_SPEED:
            if (g_set_speed_callback && g_rx_len == 1) {
                g_set_speed_callback(g_rx_buffer[0]);
            }
            break;

        case HOSTCOMM_CMD_SET_TARGET_YAW:
            if (g_set_target_yaw_callback && g_rx_len == sizeof(float)) {
                float yaw_val;
                memcpy(&yaw_val, g_rx_buffer, sizeof(float));
                g_set_target_yaw_callback(yaw_val);
            }
            break;

        case HOSTCOMM_CMD_SET_PID_PARAMS:
            if (g_set_pid_params_callback && g_rx_len == 3 * sizeof(float)) {
                float kp, ki, kd;
                memcpy(&kp, &g_rx_buffer[0], sizeof(float));
                memcpy(&ki, &g_rx_buffer[4], sizeof(float));
                memcpy(&kd, &g_rx_buffer[8], sizeof(float));
                g_set_pid_params_callback(kp, ki, kd);
            }
            break;
            
        default:
            // 未知命令
            break;
    }
}


/**
 * @brief 使用状态机处理单个接收字节
 */
static void process_byte(uint8_t byte)
{
    switch (g_rx_state) {
        case STATE_WAIT_HEADER_0:
            if (byte == HOSTCOMM_HEADER_0) {
                g_rx_state = STATE_WAIT_HEADER_1;
            }
            break;

        case STATE_WAIT_HEADER_1:
            if (byte == HOSTCOMM_HEADER_1) {
                g_rx_state = STATE_READ_CMD;
            } else {
                g_rx_state = STATE_WAIT_HEADER_0; // 序列错误，重置状态机
            }
            break;

        case STATE_READ_CMD:
            g_rx_cmd = byte;
            g_calculated_checksum = byte; // 开始计算校验和
            g_rx_state = STATE_READ_LEN;
            break;

        case STATE_READ_LEN:
            g_rx_len = byte;
            g_calculated_checksum += byte;
            g_rx_count = 0;
            if (g_rx_len == 0) {
                g_rx_state = STATE_READ_CHECKSUM; // 没有数据载荷
            } else if (g_rx_len >= sizeof(g_rx_buffer)) {
                g_rx_state = STATE_WAIT_HEADER_0; // 数据长度超长，帧错误
            }
            else {
                g_rx_state = STATE_READ_DATA;
            }
            break;

        case STATE_READ_DATA:
            g_rx_buffer[g_rx_count] = byte;
            g_calculated_checksum += byte;
            g_rx_count++;
            if (g_rx_count >= g_rx_len) {
                g_rx_state = STATE_READ_CHECKSUM;
            }
            break;

        case STATE_READ_CHECKSUM:
            if (byte == g_calculated_checksum) {
                // 校验成功，处理数据包
                process_packet();
            }
            // 不论成功与否，一帧处理完毕，重置状态机
            g_rx_state = STATE_WAIT_HEADER_0;
            break;

        default:
            g_rx_state = STATE_WAIT_HEADER_0;
            break;
    }
}

/**
 * @brief 接收处理函数，应在主循环中定期调用
 */
void HostComm_ReceiveHandler(void)
{
    uint8_t byte;
    // 循环读取UART接收缓冲区中的所有字节
    while (UART_XDS_PC_ReceiveByte(g_host_uart_handle, &byte) == UART_XDS_PC_STATUS_SUCCESS) {
        process_byte(byte);
    }
} 